%
% getPosCartesian(humanData) : returns x and y position vectors of the 
%                              specified human-data sensors.
%
% arguments:      humanData - Contains raw human data and sensor indices
%
% return values : x         - vectors of x positions of specified 
%                             human-data sensors
%                 y         - vectors of y positions of specified
%                             human-data sensors
%

function [x,y,z] = getPosCartesian(humanData)
% 0 13- Chest front             1  
% 1  6- Chest back              2  
% 4  9- Left Hip                3
% 5  5- Left Knee               4
% 6  14- Right Thigh             5
% 13  8- Left Thigh             6
% 14  7- Belly Button Front     7
% 20  20- Left Shin              8
% 21  21- Left Outside Ankle     9
% 22  24- Left  Metatarsal       10
% 23  23- Left Inside Ankle      11
% 24  22- Left Heel              12
% 25  25- Right Hip              13
% 26  26- Right Knee             14
% 27  27- Right Shin             15
% 28  28- Right Outside Ankle    16
% 29  31- Right Metatarsal       17
% 30  30- Right Inside Ankle     18
% 31  29- Right heel             19
%  [0 1 4 5 6 13 14 20 21 22 23 24 25 26 27 28 29 30 31];
%  [13, 6, 9, 5, 14, 8, 7, 20, 21, 24, 23, 22, 25, 26, 27, 28, 31, 30, 29]; 
%Extract All Data
for i = 1:length(humanData.index)
    j(i) = humanData.index(i)*3+1;
    xpos(:,i) = humanData.data(:,j(i));
    ypos(:,i) = humanData.data(:,j(i)+1);
    zpos(:,i) = humanData.data(:,j(i)+2);
end

%Shift based on min heel
offset = min(ypos(:,12));
ypos = ypos - offset;

%Convert from mm to m
xpos = xpos/1000;
ypos = ypos/1000;
zpos = zpos/1000;

%Label each sensor
x.rheel = xpos(:,19);
y.rheel = ypos(:,19);
z.rheel = zpos(:,19);
x.lheel = xpos(:,12);
y.lheel = ypos(:,12);
z.lheel = zpos(:,12);

x.rtoe = xpos(:,17);
y.rtoe = ypos(:,17);
z.rtoe = zpos(:,17);
x.ltoe = xpos(:,10);
y.ltoe = ypos(:,10);
z.ltoe = zpos(:,10);

x.rknee = xpos(:,14);
y.rknee = ypos(:,14);
z.rknee = zpos(:,14);
x.lknee = xpos(:,4);
y.lknee = ypos(:,4);
z.lknee = zpos(:,4);

x.rthigh = xpos(:,5);
y.rthigh = ypos(:,5);
z.rthigh = zpos(:,5);

x.rhip = xpos(:,13);
y.rhip = ypos(:,13);
z.rhip = zpos(:,13);
x.lhip = xpos(:,3);
y.lhip = ypos(:,3);
z.lhip = zpos(:,3);

%Averages of the ankles, hips, and torso
x.rankle_avg = (xpos(:,16)+xpos(:,18))/2;
y.rankle_avg = (ypos(:,16)+ypos(:,18))/2;
z.rankle_avg = (zpos(:,16)+zpos(:,18))/2;
x.lankle_avg = (xpos(:,9)+xpos(:,11))/2;
y.lankle_avg = (ypos(:,9)+ypos(:,11))/2;
z.lankle_avg = (zpos(:,9)+zpos(:,11))/2;

% x.lankle = xpos(:,11);
% y.lankle = ypos(:,11);
    
x.hip_avg = (xpos(:,3)+xpos(:,13))/2;
y.hip_avg = (ypos(:,3)+ypos(:,13))/2;
z.hip_avg = (zpos(:,3)+zpos(:,13))/2;
    
x.torso = (xpos(:,1)+xpos(:,2))/2;
y.torso = (ypos(:,1)+ypos(:,2))/2;
z.torso = (zpos(:,1)+zpos(:,2))/2;
    
end